package star.crust.cartspace.framework;


import star.core.util.geometry.HTransform;

public class CheckCollision {

	private CrtBody bodyA = null;
	private CrtBody bodyB = null;
	private int RAPID_num_box_tests = 0;
	private int RAPID_num_contacts = 0;

	public CheckCollision(CrtBody bodyA, CrtBody bodyB) {
		this.bodyA = bodyA;
		this.bodyB = bodyB;
	}

	/**
	 * A seconda del volume che compone la gerarchia
	 * il metodo richiama il la procedura di verifica
	 * della collisione appropriata.
	 * @return numero di collisioni
	 */
	public boolean checkCollisions() {
		CrtBoundingTree btA = bodyA.getBoundingTree();
		CrtBoxing boxA = btA.getBox();
		String type = boxA.getType();

		if (type == "OBB") {
			HTransform htA = bodyA.getPosition();
			CrtBoundingTree btB = bodyB.getBoundingTree();
			HTransform htB = bodyB.getPosition();

			float[][] R1 = CrtObbMath.HTransform2R(htA);
			float[] T1 = CrtObbMath.HTransform2T(htA);
			float[][] R2 = CrtObbMath.HTransform2R(htB);
			float[] T2 = CrtObbMath.HTransform2T(htB);

			this.RAPID_num_box_tests = 0;
			this.RAPID_num_contacts = 0;
			int RAPID_FIRST_CONTACT = 2;

			boolean collision_ok = RAPID_Collide(R1, T1, btA, R2, T2, btB, RAPID_FIRST_CONTACT);

			System.out.println("Numero di box testati: "+RAPID_num_box_tests);
			System.out.println("Numero dei contatti: "+RAPID_num_contacts);
			return collision_ok;
		}
		else if (type == "AABB") {
			return checkCollisionAabb(bodyA,bodyB);
		}
		return false;

	}

	/**
	 * Metodo per la verifica delle collisioni per
	 * la gerarchia AABB
	 * @param bta
	 * @param btb
	 * @return numero delle collisioni
	 */
	public boolean checkCollisionAabb(CrtBody bta, CrtBody btb)
	{
		return bta.intersect(bta, btb);
	}

	/**
	 * Questi metodo, chiamato ricorsivamente,
	 * verifica la presenza di collisioni tra 
	 * le due gerarchie OBB
	 */
	private boolean collide_recursive(CrtBoundingTree bt1, CrtBoundingTree bt2, float[][] R, float[] T, float s, float RAPID_ms, float[][] RAPID_mR, float[] RAPID_mT, int flag) {
		float[] d = new float[3];
		boolean rc = false;

		if (true)
		{
			if (flag == 2 && (RAPID_num_contacts > 0)) {
				return CrtObbMath.RAPID_OK;
			}

			RAPID_num_box_tests++;

			int f1;

			boolean b1_leaf = (bt1.getLeftTree() == null && bt1.getRightTree() == null);
			boolean b2_leaf = (bt2.getLeftTree() == null && bt2.getRightTree() == null);

			CrtObb b1 = (CrtObb) bt1.getBox();
			CrtObb b2 = (CrtObb) bt1.getBox();
			CrtBoundingTree b1_P = bt1.getLeftTree(), b1_N = bt1.getRightTree();
			CrtBoundingTree b2_P = bt2.getLeftTree(), b2_N = bt2.getRightTree();
			float[] b1_d = b1.getVector();
			float[] b2_d = b2.getVector();
			d[0] = s * b2_d[0];
			d[1] = s * b2_d[1];
			d[2] = s * b2_d[2];
			f1 = obb_disjoint(R, T, b1_d, d);

			if (f1 != 0) 
			{
				return CrtObbMath.RAPID_OK;
			}

			if (b1_leaf && b2_leaf) 
			{
				boolean ret = b1.tri_contact(b2, RAPID_ms, RAPID_mR, RAPID_mT);
				if (ret) RAPID_num_contacts++;
				return ret;
			}

			float[] U=new float[3];
			float[][] cR=new float[3][3];
			float[] cT=new float[3];
			float cs = 0;

			if (b2_leaf || (!b1_leaf && (b1.size() > b2.size())))
			{

				CrtObb box1_P = (CrtObb) b1_P.getBox();
				CrtObb box1_N = (CrtObb) b1_N.getBox();
				float[][] b1_P_pR = box1_P.getpR();
				float[] b1_P_pT = box1_P.getpT();
				float[][] b1_N_pR = box1_N.getpR();
				float[] b1_N_pT = box1_N.getpT();

				cR = CrtObbMath.MTxM(b1_N_pR, R); 
				U = CrtObbMath.VmV(T, b1_N_pT); cT = CrtObbMath.MTxV(b1_N_pR, U);
				cs = s;

				rc = collide_recursive(b1_N, bt2, cR, cT, cs, RAPID_ms, RAPID_mR, RAPID_mT, flag);
				if (rc != CrtObbMath.RAPID_OK) return rc;

				cR = CrtObbMath.MTxM(b1_P_pR, R); 
				U = CrtObbMath.VmV(T, b1_P_pT); cT = CrtObbMath.MTxV(b1_P_pR, U);
				cs = s;

				rc = collide_recursive(b1_P, bt2, cR, cT, cs, RAPID_ms, RAPID_mR, RAPID_mT, flag);
				if (rc != CrtObbMath.RAPID_OK) return rc;

				return CrtObbMath.RAPID_OK;
			}
			else 
			{

				CrtObb box2_P = (CrtObb) b2_P.getBox();
				CrtObb box2_N = (CrtObb) b2_N.getBox();
				float[][] b2_P_pR = box2_P.getpR();
				float[] b2_P_pT = box2_P.getpT();
				float[][] b2_N_pR = box2_N.getpR();
				float[] b2_N_pT = box2_N.getpT();

				cR = CrtObbMath.MxM(R, b2_N_pR); cT = CrtObbMath.sMxVpV(s, R, b2_N_pT, T);
				cs = s;

				rc = collide_recursive(bt1, b2_N, cR, cT, cs, RAPID_ms, RAPID_mR, RAPID_mT, flag);
				if (rc != CrtObbMath.RAPID_OK) return rc;

				cR = CrtObbMath.MxM(R, b2_P_pR); cT = CrtObbMath.sMxVpV(s, R, b2_P_pT, T);
				cs = s;

				rc = collide_recursive(bt1, b2_P, cR, cT, cs, RAPID_ms, RAPID_mR, RAPID_mT, flag);
				if (rc != CrtObbMath.RAPID_OK) return rc;

				return CrtObbMath.RAPID_OK;
			}
		}

		return CrtObbMath.RAPID_OK;
	}

	private boolean RAPID_Collide(float[][] R1, float[] T1, float s1, CrtBoundingTree RAPID_model1, float[][] R2, float[] T2, float s2, CrtBoundingTree RAPID_model2, int flag) {
		CrtObb b1 = (CrtObb) RAPID_model1.getBox();
		CrtObb b2 = (CrtObb) RAPID_model2.getBox();

		float[][] tR1 = new float[3][3];
		float[][] tR2 = new float[3][3];
		float[] tT1 = new float[3];
		float[] tT2 = new float[3];
		float[][] R = new float[3][3];
		float[] T = new float[3];
		float[] U = new float[3];
		float s;

		float[][] b1_pR = b1.getpR();
		float[] b1_pT= b1.getpT();
		float[][] b2_pR= b2.getpR();
		float[] b2_pT= b2.getpT();

		float RAPID_ms = 0;
		float[][] RAPID_mR  = new float[3][3];
		float[] RAPID_mT  = new float[3];

		tR1 = CrtObbMath.MxM(R1, b1_pR);
		tT1 = CrtObbMath.sMxVpV(s1, R1, b1_pT, T1);
		tR2 = CrtObbMath.MxM(R2, b2_pR);
		tT2 = CrtObbMath.sMxVpV(s2, R2, b2_pT, T2);

		R[0][0] = (tR1[0][0] * tR2[0][0] +

				tR1[1][0] * tR2[1][0] +

				tR1[2][0] * tR2[2][0]);

		R[1][0] = (tR1[0][1] * tR2[0][0] +

				tR1[1][1] * tR2[1][0] +

				tR1[2][1] * tR2[2][0]);

		R[2][0] = (tR1[0][2] * tR2[0][0] +

				tR1[1][2] * tR2[1][0] +

				tR1[2][2] * tR2[2][0]);

		R[0][1] = (tR1[0][0] * tR2[0][1] +

				tR1[1][0] * tR2[1][1] +

				tR1[2][0] * tR2[2][1]);

		R[1][1] = (tR1[0][1] * tR2[0][1] +

				tR1[1][1] * tR2[1][1] +

				tR1[2][1] * tR2[2][1]);

		R[2][1] = (tR1[0][2] * tR2[0][1] +

				tR1[1][2] * tR2[1][1] +

				tR1[2][2] * tR2[2][1]);

		R[0][2] = (tR1[0][0] * tR2[0][2] +

				tR1[1][0] * tR2[1][2] +

				tR1[2][0] * tR2[2][2]);

		R[1][2] = (tR1[0][1] * tR2[0][2] +

				tR1[1][1] * tR2[1][2] +

				tR1[2][1] * tR2[2][2]);

		R[2][2] = (tR1[0][2] * tR2[0][2] +

				tR1[1][2] * tR2[1][2] +

				tR1[2][2] * tR2[2][2]);


		U = CrtObbMath.VmV(tT2, tT1);  T = CrtObbMath.sMTxV(1/s1, tR1, U);

		s = s2/s1;


		RAPID_mR = CrtObbMath.MTxM(R2, R1);
		U = CrtObbMath.VmV(T1, T2);  RAPID_mT = CrtObbMath.sMTxV(1/s2, R2, U);
		RAPID_ms = s1/s2;

		return collide_recursive(RAPID_model1, RAPID_model2, R, T, s, RAPID_ms, RAPID_mR, RAPID_mT, flag);
	}



	private boolean RAPID_Collide(float[][] R1, float[] T1, CrtBoundingTree RAPID_model1, float[][] R2, float[] T2, CrtBoundingTree RAPID_model2, int flag)
	{
		return RAPID_Collide(R1, T1, 1, RAPID_model1, R2, T2, 1, RAPID_model2, flag);
	}

	/**
	 * Il test verfica se i due volumi OBB sono sovrapposti
	 * restituendo 0 se non lo sono o 1 altrimenti.
	 */
	private int obb_disjoint(float[][] B, float[] T, float[] a, float[] b)
	{
		float t, s;
		boolean r;
		float[][] Bf = new float[3][3];
		float reps = (float)1e-6;

		Bf[0][0] = CrtObbMath.myfabs(B[0][0]);  Bf[0][0] += reps;
		Bf[0][1] = CrtObbMath.myfabs(B[0][1]);  Bf[0][1] += reps;
		Bf[0][2] = CrtObbMath.myfabs(B[0][2]);  Bf[0][2] += reps;
		Bf[1][0] = CrtObbMath.myfabs(B[1][0]);  Bf[1][0] += reps;
		Bf[1][1] = CrtObbMath.myfabs(B[1][1]);  Bf[1][1] += reps;
		Bf[1][2] = CrtObbMath.myfabs(B[1][2]);  Bf[1][2] += reps;
		Bf[2][0] = CrtObbMath.myfabs(B[2][0]);  Bf[2][0] += reps;
		Bf[2][1] = CrtObbMath.myfabs(B[2][1]);  Bf[2][1] += reps;
		Bf[2][2] = CrtObbMath.myfabs(B[2][2]);  Bf[2][2] += reps;


		r = true;

		t = CrtObbMath.myfabs(T[0]);

		r &= (t <= 
			(a[0] + b[0] * Bf[0][0] + b[1] * Bf[0][1] + b[2] * Bf[0][2]));
		if (!r) return 1;

		s = T[0]*B[0][0] + T[1]*B[1][0] + T[2]*B[2][0];
		t = CrtObbMath.myfabs(s);

		r &= ( t <=
			(b[0] + a[0] * Bf[0][0] + a[1] * Bf[1][0] + a[2] * Bf[2][0]));
		if (!r) return 2;

		t = CrtObbMath.myfabs(T[1]);

		r &= ( t <= 
			(a[1] + b[0] * Bf[1][0] + b[1] * Bf[1][1] + b[2] * Bf[1][2]));
		if (!r) return 3;

		t = CrtObbMath.myfabs(T[2]);

		r &= ( t <= 
			(a[2] + b[0] * Bf[2][0] + b[1] * Bf[2][1] + b[2] * Bf[2][2]));
		if (!r) return 4;

		s = T[0]*B[0][1] + T[1]*B[1][1] + T[2]*B[2][1];
		t = CrtObbMath.myfabs(s);

		r &= ( t <=
			(b[1] + a[0] * Bf[0][1] + a[1] * Bf[1][1] + a[2] * Bf[2][1]));
		if (!r) return 5;

		s = T[0]*B[0][2] + T[1]*B[1][2] + T[2]*B[2][2];
		t = CrtObbMath.myfabs(s);

		r &= ( t <=
			(b[2] + a[0] * Bf[0][2] + a[1] * Bf[1][2] + a[2] * Bf[2][2]));
		if (!r) return 6;

		s = T[2] * B[1][0] - T[1] * B[2][0];
		t = CrtObbMath.myfabs(s);

		r &= ( t <= 
			(a[1] * Bf[2][0] + a[2] * Bf[1][0] +
					b[1] * Bf[0][2] + b[2] * Bf[0][1]));
		if (!r) return 7;

		s = T[2] * B[1][1] - T[1] * B[2][1];
		t = CrtObbMath.myfabs(s);

		r &= ( t <=
			(a[1] * Bf[2][1] + a[2] * Bf[1][1] +
					b[0] * Bf[0][2] + b[2] * Bf[0][0]));
		if (!r) return 8;

		s = T[2] * B[1][2] - T[1] * B[2][2];
		t = CrtObbMath.myfabs(s);

		r &= ( t <=
			(a[1] * Bf[2][2] + a[2] * Bf[1][2] +
					b[0] * Bf[0][1] + b[1] * Bf[0][0]));
		if (!r) return 9;

		s = T[0] * B[2][0] - T[2] * B[0][0];
		t = CrtObbMath.myfabs(s);

		r &= ( t <=
			(a[0] * Bf[2][0] + a[2] * Bf[0][0] +
					b[1] * Bf[1][2] + b[2] * Bf[1][1]));
		if (!r) return 10;

		s = T[0] * B[2][1] - T[2] * B[0][1];
		t = CrtObbMath.myfabs(s);

		r &= ( t <=
			(a[0] * Bf[2][1] + a[2] * Bf[0][1] +
					b[0] * Bf[1][2] + b[2] * Bf[1][0]));
		if (!r) return 11;

		s = T[0] * B[2][2] - T[2] * B[0][2];
		t = CrtObbMath.myfabs(s);

		r &= (t <=
			(a[0] * Bf[2][2] + a[2] * Bf[0][2] +
					b[0] * Bf[1][1] + b[1] * Bf[1][0]));
		if (!r) return 12;

		s = T[1] * B[0][0] - T[0] * B[1][0];
		t = CrtObbMath.myfabs(s);

		r &= (t <=
			(a[0] * Bf[1][0] + a[1] * Bf[0][0] +
					b[1] * Bf[2][2] + b[2] * Bf[2][1]));
		if (!r) return 13;

		s = T[1] * B[0][1] - T[0] * B[1][1];
		t = CrtObbMath.myfabs(s);

		r &= ( t <=
			(a[0] * Bf[1][1] + a[1] * Bf[0][1] +
					b[0] * Bf[2][2] + b[2] * Bf[2][0]));
		if (!r) return 14;

		s = T[1] * B[0][2] - T[0] * B[1][2];
		t = CrtObbMath.myfabs(s);

		r &= ( t <=
			(a[0] * Bf[1][2] + a[1] * Bf[0][2] +
					b[0] * Bf[2][1] + b[1] * Bf[2][0]));
		if (!r) return 15;

		return 0;
	}



}